/**
  ******************************************************************************
  * @file           : rotation_matrix_drv.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/3/7
  ******************************************************************************
  */



#include "rotation_matrix_drv.h"

#define s arm_sin_f32
#define c arm_cos_f32

void euler_zyx2rot_mat(float z, float y, float x, float mat[9]){
    mat[0] = c(z) * c(y);
    mat[1] = c(z) * s(y) * s(x) - s(z) * c(x);
    mat[2] = c(z) * s(y) * c(x) + s(z) * s(x);

    mat[3] = s(z) * c(y);
    mat[4] = s(z) * s(y) * s(x) + c(z) * c(x);
    mat[5] = s(z) * s(y) * c(x) - c(z) * s(x);

    mat[6] = -s(y);
    mat[7] = c(y) * s(x);
    mat[8] = c(y) * c(x);
    //使用3*3矩阵
}

void euler_xyz2rot_mat(float x, float y, float z, float mat[9]){
    mat[0] = c(z) * c(y);
    mat[1] = -s(z) * c(y);
    mat[2] = s(y);

    mat[3] = c(z) * s(y) * s(x) + s(z) * c(x);
    mat[4] = -s(z) * s(y) * s(x) + c(z) * c(x);
    mat[5] = -c(y) * s(x);

    mat[6] = -c(z) * s(y) * c(x) + s(z) * s(x);
    mat[7] = s(z) * s(y) * c(x) + c(z) * s(x);
    mat[8] = c(y) * c(x);
    //使用3*3矩阵
}

void mat_rot_y(float a,float mat[9]){
    mat[0] = c(a);
    mat[1] = 0;
    mat[2] = s(a);
    mat[3] = 0;
    mat[4] = 1;
    mat[5] = 0;
    mat[6] = -s(a);
    mat[7] = 0;
    mat[8] = c(a);
}

//绕x轴旋转theta角度旋转矩阵
void rotate_x_rotation_matrix(float theta,float matrix[9]){
    matrix[0] = 1;
    matrix[1] = 0;
    matrix[2] = 0;

    matrix[3] = 0;
    matrix[4] = c(theta);
    matrix[5] = -s(theta);

    matrix[6] = 0;
    matrix[7] = s(theta);
    matrix[8] = c(theta);
}

void rotate_z_rotation_matrix(float theta,float matrix[9]){
    matrix[0] = c(theta);
    matrix[1] = -s(theta);
    matrix[2] = 0;

    matrix[3] = s(theta);
    matrix[4] = c(theta);
    matrix[5] = 0;

    matrix[6] = 0;
    matrix[7] = 0;
    matrix[8] = 1;
}

void matXmat(float A[9],float B[9],float C[9]){
    arm_matrix_instance_f32 mat_A;
    arm_matrix_instance_f32 mat_B;
    arm_matrix_instance_f32 mat_C;
    arm_mat_init_f32(&mat_A,3,3,A);
    arm_mat_init_f32(&mat_B,3,3,B);
    arm_mat_init_f32(&mat_C,3,3,C);
    arm_mat_mult_f32(&mat_A,&mat_B,&mat_C);
}

void matXvec(float A[9],float B[3],float C[3]){
    arm_matrix_instance_f32 mat_A;
    arm_matrix_instance_f32 mat_B;
    arm_matrix_instance_f32 mat_C;
    arm_mat_init_f32(&mat_A,3,3,A);
    arm_mat_init_f32(&mat_B,3,1,B);
    arm_mat_init_f32(&mat_C,3,1,C);
    arm_mat_mult_f32(&mat_A,&mat_B,&mat_C);
}

#undef s
#undef c
